﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.Foundation;

namespace Drifting.MotionPlanning.Systems
{
    public class Point3D : BaseSystem
    {
        public Point3D()
            : base("../../../MotionPlanning/Resources/Point3D.kdl")
        {
        }

        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            throw new NotImplementedException();
        }

        override public bool Propagate(MemoryPad pad, double[] q_start, double[] u, double duration, ref double[] q_end, ref double step_cost)
        {
            double dx = u[0];
            double dy = u[1];
            double dz = u[2];
            double length = Math.Sqrt(dx * dx + dy * dy + dz * dz);

            step_cost = duration;

            if (q_end == null)
                q_end = new double[DOF_State];
            q_end[0] = q_start[0] + dx / length * duration;
            q_end[1] = q_start[1] + dy / length * duration;
            q_end[2] = q_start[2] + dz / length * duration;

            return inStateSpace(q_end);
        }
    }
}
